﻿#include "AstarNode.h"

#include <functional>

bool nodecmp(const AstarNode* n1, const AstarNode* n2)
{
	return (n1->hn_ + n1->gn_) < (n2->gn_ + n2->hn_);
}

//占位声明

int AstarNode::dest_x_;
int AstarNode::dest_y_;
int AstarNode::sizey;
int AstarNode::sizex;
int AstarNode::start_x_;
int AstarNode::start_y_;
int AstarNode::smallfn;
std::map<Position,AstarNode*> AstarNode::allNodes;
std::list<AstarNode*> AstarNode::closedNodes;

//std::multiset<AstarNode* ,std::function<bool(const AstarNode* n1, const AstarNode* n2)>> AstarNode::openNodes(nodecmp);

std::list<AstarNode*> AstarNode::openNodes;

AstarNode* AstarNode::getNode(int x,int y)
{
	const auto node = allNodes.find(Position(x, y));
	if (node !=allNodes.end())
	{
		return node->second;
	}
	return nullptr;
}

bool AstarNode::operator<(const AstarNode& other) const
{
	return this->getFn() < other.getFn();
}



int AstarNode::getFn() const
{
	return  gn_ + hn_;
}



void AstarNode::findNext()
{
	isclosed_ = true;
	

	//std::cout<<"opennode_size:"<<openNodes.size() << "current:" << this->x_ << "," << this->y_ << "---fn:" << getFn() << "\n";
	
	
	if (dest_x_==x_&&dest_y_== y_)
	{
		//std::cout << "found end " << '\n';
		std::cout << x_ << "," << y_ << "\n";
		auto fa = this->parentNode;
		while (fa != nullptr)
		{
			std::cout << fa->x_ << "," << fa->y_ << "\n";
			fa = fa->parentNode;
		}
		
		return;
	}
	if (isvalied(1, 0)) //right
	{
		findchild(1, 0);
	}
	if (isvalied(0,1)) //up
	{
		findchild(0, 1);
	}
	if (isvalied(-1, 0)) //left
	{
		findchild(-1, 0);
	}if (isvalied(0, -1)) //down
	{
		findchild(0, -1);
	}
	for (auto i = openNodes.begin(); i!=openNodes.end(); ++i)
	{	
		if (*i == this)
		{
			openNodes.erase(i);
			break;
		}
	}
	auto smallestFn = (*openNodes.begin())->getFn();
	AstarNode* nodeptr = *openNodes.begin();
	for (auto i = openNodes.begin(); i != openNodes.end(); ++i)
	{
		if ((*i)->getFn()<smallestFn)
		{
			smallestFn = (*i)->getFn();
			nodeptr = *i;
		}
	}
	//std::cout << "expend:" << nodeptr->getFn() << "\n";
	nodeptr->findNext();
}


AstarNode* AstarNode::findchild(int offsetx ,int offsety)
{
	auto node = getNode(x_+ offsetx, y_ + offsety);
	if (node == nullptr)
	{
		node = new AstarNode(x_ + offsetx, y_ + offsety);
		node->parentNode = this;
		node->gn_ = gn_ + 1;

		openNodes.push_front(node);
		
		//std::cout << node->x_<<","<<node->y_ << "add\n";
		
	}
	else
	{
		if (node->gn_ > gn_&& node->isclosed_)
		{
			std::cout <<"contact"<< node->x_<<","<<node->y_<<"--"<<x_<<","<<y_<<"\n";
			std::cout << "fn this" << getFn() << "==fn contact" << node->getFn()<< "\n";
			node->parentNode = this;
			node->gn_ = gn_ + 1;
		}
	}
	if (node->isclosed_)
	{
		return nullptr;
	}
	
	return  node;
}


bool AstarNode::isvalied(int x, int y) const
{
	if (y + y_ == 3 && x + x_ == 10)
	{
		return true;
	}
	if (y + y_ == 0 && x + x_ == 10)
	{
		return true;
	}
	if (y + y_ == 5 && x + x_ == 5)
	{
		return true;
	}
	if (y + y_ == 10 && x + x_ == 10)
	{
		std::cout << "pass\n";
		return true;
	}
	if (x_ + x < 0 || x_ + x >= sizex || y_ + y < 0 || y_ + y >= sizey)
	{
		return  false;
	}
	if (y + y_ == 5)
	{
		return false;
	}
	if (x + x_ == 10)
	{
		return false;
	}
	
	return  true;
}